/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Just reads a MaCI pose and ranging files and stores it in "6D format" and in Sensor Frame
* 
* - very quick hack 
* 
*/
#include <math.h>
#include <vector>
#include "owndebug.h"
#include "ownutils.h"
#include "gimutils.h"
#include "CScanMeasurementSet.h"


int main(int argc, char *argv[]){
	
	if(argc !=3){
		fprintf(stderr,"\nUsage: ./poseConvert [in_position_file] [in_ranging_file]\n");
		exit(1);
	}
	char *positionFile = argv[1];
	char *rangingFile = argv[2];
	CScanMeasurementSet set(positionFile,rangingFile);
	
	Grid::pose p;
	for(int i=0;i<set.size();i++){
		set.poses[i].a = M_PI/2.0 - set.poses[i].a;
	}
	set.saveMaCIPositionFILE("Reza_Position_Matched.txt",
											 "Reza",
											 "Weird-Coordinates");
	
	return 0;
}